/*============================================================================*/
/*
SimpleRTOS - Very simple RTOS for Microcontrollers - PIC18 port
v2.00 (2014-01-21)
isaacbavaresco@yahoo.com.br
*/
/*============================================================================*/
/*
 Copyright (c) 2007-2014, Isaac Marino Bavaresco
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:
     * Redistributions of source code must retain the above copyright
       notice, this list of conditions and the following disclaimer.
     * Neither the name of the author nor the
       names of its contributors may be used to endorse or promote products
       derived from this software without specific prior written permission.

 THIS SOFTWARE IS PROVIDED BY THE AUTHOR ''AS IS'' AND ANY
 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
/*============================================================================*/
#if			defined __XC8__ || defined __18CXX
/*============================================================================*/
#include <string.h>
#include <errno.h>
#include "PortPIC18.h"
#include "SimpleRTOSInternals.h"
/*============================================================================*/
void TickHook( void );
/*============================================================================*/
static void Switch( void )
	{
	PIR1bits.TMR1IF	= 0; 	/* clear the interrupt flag */
	PIR3bits.CCP3IF	= 0;

	SystemTick++;

	CheckDelayList();

	/* The current task ran for at least one full time slice... */
	if( AlreadySwitched == 0 && CurrentTask->Priority == HighestReadyPriority )
		/* ... it doesn't deserve an additional time slice. */
		ReadyTasks[HighestReadyPriority]
			= ReadyTasks[HighestReadyPriority]->Next;

	CurrentTask			= ReadyTasks[HighestReadyPriority];

	/*
	The upcoming task will run from the very beginning of its time slice,
	at the end of this slice it will be switched off.
	*/
	AlreadySwitched		= 0;

	TickHook();
	}
/*============================================================================*/
void ContextInit( context_t *Context, unsigned char *Stack, unsigned long StackSize, void *Parameter, void (*TaskFunc)( void* ), unsigned int Priority )
    {
	unsigned long	FuncAddress;
	unsigned char	*TopOfStack;

	unsigned char	*p, nBytes;

	memset( Context, 0x00, sizeof( context_t ));

	FuncAddress			= (unsigned short)TaskFunc;

	TopOfStack			= Stack;

	/*------------------------------------------------------------------------*/

	/* Simulate how the stack would look after a call to vPortYield() generated
	by the compiler. 

	First store the function parameters.  This is where the task will expect to
	find them when it starts running. */
	*TopOfStack++		= (unsigned short)Parameter; /* >> 0; */

	*TopOfStack++		= (unsigned short)Parameter >> 8; 

	/* Next we just leave a space.  When a context is saved the stack pointer
	is incremented before it is used so as not to corrupt whatever the stack
	pointer is actually pointing to.  This is especially necessary during 
	function epilogue code generated by the compiler. */
	*TopOfStack++		= 0x44;

	/* Next are all the registers that form part of the task context. */
	
	*TopOfStack++		= 0x66;	/* WREG. */

	*TopOfStack++		= 0xcc;	/* Status. */

	/* INTCON is saved with interrupts enabled. */
	*TopOfStack++		= 0xc0;	/* INTCON */
	*TopOfStack++		= 0x11; /* BSR. */
	*TopOfStack++		= 0x22; /* FSR2L. */
	*TopOfStack++		= 0x33; /* FSR2H. */
	*TopOfStack++		= 0x44; /* FSR0L. */
	*TopOfStack++		= 0x55; /* FSR0H. */
	*TopOfStack++		= 0x66; /* TABLAT. */
	*TopOfStack++		= 0x00; /* TBLPTRU. */
	*TopOfStack++		= 0x88; /* TBLPTRUH. */
	*TopOfStack++		= 0x99; /* TBLPTRUL. */
	*TopOfStack++		= 0xaa; /* PRODH. */
	*TopOfStack++		= 0xbb; /* PRODL. */
	*TopOfStack++		= 0x00; /* errno */
	*TopOfStack++		= 0x00; /* PCLATU. */
	*TopOfStack++		= 0x00; /* PCLATH. */

	/* The only function return address so far is the address of the 
	task. */

	/* TOS low. */
	*TopOfStack++		= FuncAddress; /* >>  0; */
	/* TOS high. */
	*TopOfStack++		= FuncAddress >>  8;
	/* TOS even higher. */
	*TopOfStack++		= FuncAddress >> 16;

	*TopOfStack++		= 1;

	/* Next the .tmpdata section. */
	for( nBytes = GetTMPDATALen(), p = GetTMPDATAStrt(); nBytes; nBytes--, TopOfStack++, p++ )
		*TopOfStack = *p;

	/* Next the MATH_DATA section. */
	for( nBytes = GetMATHDATALen(), p = GetMATHDATAStrt(); nBytes; nBytes--, TopOfStack++, p++ )
		*TopOfStack = *p;

	/*------------------------------------------------------------------------*/

	Context->TopOfStack				= TopOfStack;

	Context->Priority				= Priority;
    Context->TDelay					= 0;
	Context->PreviousDelayedTask	= 0;
	Context->NextDelayedTask		= 0;
	Context->DelayList				= 0;

    Context->Previous				= 0;
    Context->Next					= 0;
    Context->List					= 0;
    }
/*============================================================================*/

void InitRTOS( context_t *Contexts, int Number )
	{
    unsigned int i;
    
	/* No contexts to be initialized... The RTOS is useless.*/
    if( Contexts == NULL || Number == 0 )
	    {
        FreeContexts        = NULL;
        return;
        }

	CurrentTask		= Contexts;

	for( i = 0; i < MAX_PRIORITIES; i++ )
		ReadyTasks[i]	= NULL;

	/*------------------------------------------------------------------------*/
	/* Initialize all contexts and link them to the free contexts list.*/
    FreeContexts		= Contexts;
    for( i = 0; i < Number - 1; i++, Contexts++ )
        {
        Contexts->Next	= Contexts + 1;
        Contexts->List	= &FreeContexts;
        }
    Contexts->Next		= NULL;
    Contexts->List		= &FreeContexts;

	/*------------------------------------------------------------------------*/
	}
/*============================================================================*/
intsave_t SaveAndDisableInterrupts( void )
	{
	intsave_t	Status;

	Status	= INTCON;
	INTCON	&= 0xbf;

	return Status;
	}
/*============================================================================*/
void RestoreInterrupts( intsave_t Status )
	{
	INTCON	|= Status & 0x40;
	}

/*============================================================================*/
/* 
 * Macro that pushes all the registers that make up the context of a task onto
 * the stack, then saves the new top of stack into the TCB.
 * 
 * If this is called from an ISR then the interrupt enable bits must have been 
 * set for the ISR to ever get called.  Therefore we want to save the INTCON
 * register with the enable bits forced to be set - and ForcedInterruptFlags 
 * must contain these bit settings.  This means the interrupts will again be
 * enabled when the interrupted task is switched back in.
 *
 * If this is called from a manual context switch (i.e. from a call to yield),
 * then we want to save the INTCON so it is restored with its current state,
 * and ForcedInterruptFlags must be 0.  This allows a yield from within
 * a critical section.
 *
 * The compiler uses some locations at the bottom of the memory for temporary
 * storage during math and other computations.  This is especially true if
 * 32bit data types are utilised (as they are by the scheduler).  The .tmpdata
 * and MATH_DATA sections have to be stored in there entirety as part of a task
 * context.  This macro stores from data address 0x00 to 
 * COMPILER_MANAGED_MEMORY_SIZE.  This is sufficient for the demo 
 * applications but you should check the map file for your project to ensure 
 * this is sufficient for your needs.  It is not clear whether this size is 
 * fixed for all compilations or has the potential to be program specific.
 */

#define	SaveContext( ForcedInterruptFlags )										\
{																				\
	_asm																		\
		/* Save the status and WREG registers first, as these will get modified*/	\
		/*by the operations below. */											\
		MOVFF	WREG, PREINC1													\
		MOVFF   STATUS, PREINC1													\
		/* Save the INTCON register with the appropriate bits forced if	*/		\
		/*necessary - as described above. */									\
		MOVFF	INTCON, WREG													\
		IORLW	ForcedInterruptFlags											\
		MOVFF	WREG, PREINC1													\
		/* Store the necessary registers to the stack. */						\
		MOVFF	BSR, PREINC1													\
		MOVFF	FSR2L, PREINC1													\
		MOVFF	FSR2H, PREINC1													\
		MOVFF	FSR0L, PREINC1													\
		MOVFF	FSR0H, PREINC1													\
		MOVFF	TABLAT, PREINC1													\
		MOVFF	TBLPTRU, PREINC1												\
		MOVFF	TBLPTRH, PREINC1												\
		MOVFF	TBLPTRL, PREINC1												\
		MOVFF	PRODH, PREINC1													\
		MOVFF	PRODL, PREINC1													\
																				\
		MOVFF	errno, PREINC1													\
																				\
		movff	INTCON,PRODL													\
		bcf		INTCON,7,ACCESS													\
		bcf		INTCON,6,ACCESS													\
																				\
		MOVFF	PCLATU, PREINC1													\
		MOVFF	PCLATH, PREINC1													\
																				\
		/* Store the hardware stack pointer in a temp register before we*/		\
		/*modify it. */															\
		MOVFF	STKPTR, FSR0L													\
	_endasm																		\
																				\
		/* Store each address from the hardware stack. */						\
		while( STKPTR > (unsigned char)0 )										\
		{																		\
			_asm																\
				MOVFF	TOSL, PREINC1											\
				MOVFF	TOSH, PREINC1											\
				MOVFF	TOSU, PREINC1											\
				POP																\
			_endasm																\
		}																		\
																				\
	_asm																		\
		/* Store the number of addresses on the hardware stack (from the		\
		temporary register). */													\
		MOVFF	FSR0L, PREINC1													\
																				\
		call	SaveTMP_MATH,0													\
																				\
		MOVF	PREINC1, 1, 0													\
																				\
	_endasm																		\
																				\
	/* Save the new top of the software stack in the TCB. */					\
	_asm																		\
		MOVFF	CurrentTask+0, FSR0L											\
		MOVFF	CurrentTask+1, FSR0H											\
		MOVFF	FSR1L, POSTINC0													\
		MOVFF	FSR1H, POSTINC0													\
																				\
		movff	PRODL,INTCON													\
	_endasm																		\
}
/*-----------------------------------------------------------*/

/*
 * This is the reverse of SAVE_CONTEXT.  See SAVE_CONTEXT for more
 * details.
 */
#define RestoreContext()														\
{																				\
	_asm																		\
		movff	INTCON,PRODL													\
		bcf		INTCON,7,ACCESS													\
		bcf		INTCON,6,ACCESS													\
																				\
		/* Set FSR0 to point to CurrentTask->TopOfStack. */						\
		MOVFF	CurrentTask+0, FSR0L											\
		MOVFF	CurrentTask+1, FSR0H											\
																				\
		/* De-reference FSR0 to set the address it holds into FSR1.*/			\
		/*(i.e. *( pxCurrentTCB->TopOfStack ) ). */								\
		MOVFF	POSTINC0, FSR1L													\
		MOVFF	POSTINC0, FSR1H													\
																				\
		/* How many return addresses are there on the hardware stack?  Discard*/	\
		/*the first byte as we are pointing to the next free space. */			\
		MOVFF	POSTDEC1, FSR0L													\
																				\
		call	RestoreTMP_MATH,0												\
																				\
		MOVFF	POSTDEC1, FSR0L													\
	_endasm																		\
																				\
	/* Fill the hardware stack from our software stack. */						\
	STKPTR = 0;																	\
																				\
	while( STKPTR < FSR0L )														\
	{																			\
		_asm																	\
			PUSH																\
			MOVF	POSTDEC1, 0, 0												\
			MOVWF	TOSU, 0														\
			MOVF	POSTDEC1, 0, 0												\
			MOVWF	TOSH, 0														\
			MOVF	POSTDEC1, 0, 0												\
			MOVWF	TOSL, 0														\
		_endasm																	\
	}																			\
																				\
	_asm																		\
		/* Restore the other registers forming the tasks context. */			\
		MOVFF	POSTDEC1, PCLATH												\
		MOVFF	POSTDEC1, PCLATU												\
																				\
		movff	PRODL,INTCON													\
																				\
		MOVFF	POSTDEC1, errno													\
																				\
		MOVFF	POSTDEC1, PRODL													\
		MOVFF	POSTDEC1, PRODH													\
		MOVFF	POSTDEC1, TBLPTRL												\
		MOVFF	POSTDEC1, TBLPTRH												\
		MOVFF	POSTDEC1, TBLPTRU												\
		MOVFF	POSTDEC1, TABLAT												\
		MOVFF	POSTDEC1, FSR0H													\
		MOVFF	POSTDEC1, FSR0L													\
		MOVFF	POSTDEC1, FSR2H													\
		MOVFF	POSTDEC1, FSR2L													\
		MOVFF	POSTDEC1, BSR													\
		/* The next byte is the INTCON register.  Read this into WREG as some*/	\
		/*manipulation is required. */											\
		MOVFF	POSTDEC1, WREG													\
	_endasm																		\
																				\
	/* From the INTCON register, only the interrupt enable bits form part */	\
	/*of the tasks context.  It is perfectly legitimate for another task to */	\
	/*have modified any other bits.  We therefore only restore the top two bits.*/	\
	if( WREG & 0x40 )															\
	{																			\
		_asm 																	\
			MOVFF	POSTDEC1, STATUS											\
			MOVFF	POSTDEC1, WREG												\
			/* Return enabling interrupts. */									\
			RETFIE	0															\
		_endasm																	\
	}																			\
	else																		\
	{																			\
		_asm 																	\
			MOVFF	POSTDEC1, STATUS											\
			MOVFF	POSTDEC1, WREG												\
			/* Return without effecting interrupts.  The context may have*/		\
			/*been saved from a critical region. */								\
			RETURN	0															\
		_endasm																	\
	}																			\
}

/*============================================================================*/

void U1RXHandler( void );
void U1TXHandler( void );

void ISRLow( void )
	{
	/* Was the interrupt the tick? */
	if( PIR3bits.CCP3IF )
		{
		SaveContext( 0x40 );
		if( SystemTick == 523 )
			Nop();
		Switch();
		RestoreContext();
		}
	else if( PIR1bits.RC1IF && PIE1bits.RC1IE )
		{
		SaveContext( 0x40 );
		U1RXHandler();
		RestoreContext();
		}
	else if( PIR1bits.TX1IF && PIE1bits.TX1IE )
		{
		SaveContext( 0x40 );
		U1TXHandler();
		RestoreContext();
		}
	}

/*============================================================================*/

#pragma code low_vector=0x18

static void LowISR( void )
	{		
	_asm goto ISRLow _endasm;
	}

#pragma code

/*============================================================================*/

void StartRTOS( tickcount_t TickPeriod )
	{
    SystemTick			= 0;
	CurrentTask			= ReadyTasks[HighestReadyPriority];

	CCPR3L				= TickPeriod;
	CCPR3H				= TickPeriod >> 8;

	RCONbits.IPEN		= 1;

	CCP3CONbits.CCP3M0	= 1;	/*< Compare match mode. */
	CCP3CONbits.CCP3M1	= 1;	/*< Compare match mode. */
	CCP3CONbits.CCP3M2	= 0;	/*< Compare match mode. */
	CCP3CONbits.CCP3M3	= 1;	/*< Compare match mode. */
	IPR3bits.CCP3IP		= 0;
	PIE3bits.CCP3IE		= 1;	/*< Interrupt enable. */

	INTCONbits.GIEL		= 1;
	INTCONbits.GIEH		= 1;

	TMR1H				= 0;
	TMR1L				= 0;
	T1CON				= 0x05;

	T3CON				= 0x00;

	PIR1bits.TMR1IF		= 0;
	PIE1bits.TMR1IE		= 0;

	RestoreContext();

	while( 1 )
		{}

	(void)LowISR;
    }
/*============================================================================*/

void ForceYield( void )
	{
	SaveContext( 0 );

	CurrentTask	= ReadyTasks[HighestReadyPriority];

	RestoreContext();
	}
/*============================================================================*/
void HigherPriorityAwakened( void )
	{
	CurrentTask	= ReadyTasks[HighestReadyPriority];

	RestoreContext();
	}
/*============================================================================*/
#endif	/*	defined __XC8__ || defined __18CXX */
/*============================================================================*/
